#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include "auto_navigation/planning/planner_manager.hpp"
#include "auto_navigation/planning/astar_planner.hpp"

using namespace auto_navigation::planning;

class PathPlanningNode {
public:
    PathPlanningNode() : tf_listener_(tf_buffer_) {
        ros::NodeHandle nh;
        ros::NodeHandle private_nh("~");
        
        // 初始化规划器管理器
        planner_manager_.initialize(private_nh);
        
        // 创建并注册A*规划器
        auto astar_planner = std::make_shared<AStarPlanner>();
        planner_manager_.registerPlanner(astar_planner);
        
        // 订阅地图
        map_sub_ = nh.subscribe("/map", 1, &PathPlanningNode::mapCallback, this);
        
        // 订阅目标点
        goal_sub_ = nh.subscribe("/move_base_simple/goal", 1, &PathPlanningNode::goalCallback, this);
        
        // 发布路径
        path_pub_ = nh.advertise<nav_msgs::Path>("/planning/path", 1);
        
        // 发布可视化路径
        vis_path_pub_ = nh.advertise<nav_msgs::Path>("/planning/path_vis", 1);
        
        ROS_INFO("Path Planning Node initialized");
    }
    
    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
        ROS_INFO("Received map");
        planner_manager_.setMap(*msg);
        map_received_ = true;
    }
    
    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        if (!map_received_) {
            ROS_ERROR("No map received yet");
            return;
        }
        
        ROS_INFO("Received goal: (%.2f, %.2f)", msg->pose.position.x, msg->pose.position.y);
        
        // 获取当前机器人位置
        geometry_msgs::PoseStamped current_pose;
        if (!getCurrentPose(current_pose)) {
            ROS_ERROR("Failed to get current pose");
            return;
        }
        
        ROS_INFO("Current pose: (%.2f, %.2f)", current_pose.pose.position.x, current_pose.pose.position.y);
        
        // 规划路径
        nav_msgs::Path path;
        if (planner_manager_.plan(current_pose, *msg, path)) {
            ROS_INFO("Path planning succeeded with %zu points", path.poses.size());
            
            // 发布路径
            path_pub_.publish(path);
            vis_path_pub_.publish(path);
        } else {
            ROS_ERROR("Path planning failed");
        }
    }
    
    bool getCurrentPose(geometry_msgs::PoseStamped& pose) {
        try {
            // 获取从map到base_link的变换
            geometry_msgs::TransformStamped transform_stamped = 
                tf_buffer_.lookupTransform("map", "base_link", ros::Time(0), ros::Duration(1.0));
            
            // 创建位姿
            pose.header.frame_id = "map";
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = transform_stamped.transform.translation.x;
            pose.pose.position.y = transform_stamped.transform.translation.y;
            pose.pose.position.z = transform_stamped.transform.translation.z;
            pose.pose.orientation = transform_stamped.transform.rotation;
            
            return true;
        } catch (tf2::TransformException &ex) {
            ROS_WARN("Failed to get transform: %s", ex.what());
            return false;
        }
    }
    
private:
    PlannerManager planner_manager_;
    ros::Subscriber map_sub_;
    ros::Subscriber goal_sub_;
    ros::Publisher path_pub_;
    ros::Publisher vis_path_pub_;
    bool map_received_ = false;
    
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "path_planning_node");
    PathPlanningNode node;
    ros::spin();
    return 0;
} 